#!/usr/bin/env python
# coding: utf-8
import transforms3d as tfs
import numpy as np
import math
from math import sin, cos, pi

def get_matrix_eular_radu(x,y,z,rx,ry,rz):
    rmat = tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz))
    rmat = tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])
    return rmat

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def rot2quat_minimal(m):
    quat =  tfs.quaternions.mat2quat(m[0:3,0:3])
    return quat[1:]

def quatMinimal2rot(q):
    p = np.dot(q.T,q)
    w = np.sqrt(np.subtract(1,p[0][0]))
    return tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

trans = []

def TransMatrix(x,y,z,alpha,theta,beta):
    global trans
    a1 = cos(theta)*cos(beta)
    a2 = sin(alpha)*sin(theta)*cos(beta)-cos(alpha)*sin(beta)
    a3 = cos(alpha)*sin(theta)*cos(beta)+sin(alpha)*sin(beta)
    a4 = x
    a5 = cos(theta)*cos(beta)
    a6 = sin(alpha)*sin(theta)*sin(beta)+cos(alpha)*sin(beta)
    a7 = cos(alpha)*sin(theta)*sin(beta)-sin(alpha)*cos(beta)
    a8 = y
    a9 = -sin(theta)
    a10 = sin(alpha)*cos(theta)
    a11 = cos(alpha)*cos(theta)
    a12 = z
    trans = np.array([[a1,a2,a3,a4],[a5,a6,a7,a8],[a9,a10,a11,a12],[0,0,0,1]])

gyh = [
    [
        [0.049595324217448285, -0.07250059375131356, 0.37603895615217114, math.radians(155.17196646200375), math.radians(87.69045667920177), math.radians(3.4840991991235235)],
        [0.593424093595387, 0.2178888675606022, 0.29479844432440483, math.radians(147.19083091110005), math.radians(-98.52777997809012), math.radians(2.3581572375530553)]
    ],
    [
        [0.035791509317587115, 0.03534269391764287, 0.405957050297583, math.radians(-157.4970305774787), -math.radians(81.70398998661678), math.radians(-19.959153254692392)],
        [0.6404096490776013, 0.2816626003219472, 0.2766804131718746, math.radians(-149.55894122925162), math.radians(93.0655928533858), math.radians(-23.863211826556917)]
    ],
    [
        [-0.00031417215394799437, 0.059623612925632584, 0.4782402101456777, math.radians(152.8975296074382), math.radians(72.28426202324343), math.radians(-27.596543594096573)],
        [0.5995321025898971, 0.3750802536442224, 0.21046870115509736, math.radians(149.9198491357414), math.radians(-85.1381779129862), math.radians(-26.77257429235376)]
    ]
]
hand = []
camera = []

for i in gyh:
    camerat = i[0]
    handt = i[1]
    for j in camerat:
        camera.append(j)
    for j in handt:
        hand.append(j)

Hgs,Hcs = [],[]
for i in range(0,len(hand),6):
    Hgs.append(get_matrix_eular_radu(hand[i],hand[i+1],hand[i+2],hand[i+3],hand[i+4],hand[i+5]))    
    Hcs.append(get_matrix_eular_radu(camera[i],camera[i+1],camera[i+2],camera[i+3],camera[i+4],camera[i+5]))

Hgijs = []
Hcijs = []
A = []
B = []
size = 0
for i in range(len(Hgs)):
    for j in range(i+1,len(Hgs)):
        size += 1
        Hgij = np.dot(np.linalg.inv(Hgs[j]),Hgs[i])
        Hgijs.append(Hgij)
        Pgij = np.dot(2,rot2quat_minimal(Hgij))

        Hcij = np.dot(Hcs[j],np.linalg.inv(Hcs[i]))
        Hcijs.append(Hcij)
        Pcij = np.dot(2,rot2quat_minimal(Hcij))

        A.append(skew(np.add(Pgij,Pcij)))
        B.append(np.subtract(Pcij,Pgij))
MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Pcg_  =  np.dot(np.linalg.pinv(MA),MB)
pcg_norm = np.dot(np.conjugate(Pcg_).T,Pcg_)
Pcg = np.sqrt(np.add(1,np.dot(Pcg_.T,Pcg_)))
Pcg = np.dot(np.dot(2,Pcg_),np.linalg.inv(Pcg))
Rcg = quatMinimal2rot(np.divide(Pcg,2)).reshape(3,3)


A = []
B = []
id = 0
for i in range(len(Hgs)):
    for j in range(i+1,len(Hgs)):
        Hgij = Hgijs[id]
        Hcij = Hcijs[id]
        A.append(np.subtract(Hgij[0:3,0:3],np.eye(3,3)))
        B.append(np.subtract(np.dot(Rcg,Hcij[0:3,3:4]),Hgij[0:3,3:4]))
        id += 1

MA = np.asarray(A).reshape(size*3,3)
MB = np.asarray(B).reshape(size*3,1)
Tcg = np.dot(np.linalg.pinv(MA),MB).reshape(3,)
print(tfs.affines.compose(Tcg,np.squeeze(Rcg),[1,1,1]))